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ABSTRACT 


In  the  tracking  of  a  moving  ground  target  by  small  unmanned  air  vehicle 
(UAV)  via  camera  vision,  the  target  position  and  motion  cannot  be  measured 
directly.  Two  different  types  of  filters  were  assessed  for  their  ability  to  estimate 
target  motion,  namely  target  velocity,  directional  heading  on  flat  ground  and 
distance  from  the  UAV  to  target.  The  first  filter  is  a  nonlinear  deterministic  filter 
with  stability  guarantee.  The  second  filter  is  based  on  nonlinear  Kalman  Filter 
technique.  The  application  and  performance  of  these  two  filters  are  presented, 
for  simulated  vision  based  target  tracking. 
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I.  INTRODUCTION 


A.  BACKGROUND 

The  goal  of  this  project  was  to  enhance  the  target  tracking  features  of  the 
airborne  sensor  in  support  of  the  Tactical  Network  Topology  (TNT)  experiment,  in 
which  the  Naval  Postgraduate  School  (NPS)  is  participating.  This  experiment 
assesses  the  information  flow  in  a  network  through  scenario  plays  and  gathers 
part  of  the  required  information  through  various  sensors.  The  airborne  sensor  is 
one  such  sensor  through  which  ground  target  information  can  be  collected. 
Currently  the  airborne  sensor  includes  the  Small  Unmanned  Aerial  Vehicle 
(SUAV)  equipped  with  a  pan-tilt  camera  for  target  tracking  purpose.  This  sensor 
was  previously  developed  in  NPS  as  a  system  that  incorporates  ground  target 
tracking  control  and  SUAV  guidance.  The  SUAV  to  target  distance  information 
was  used  to  guide  the  SUAV  to  fly  in  a  circular  path,  to  facilitate  continuous 
tracking  by  its  onboard  camera.  The  current  target  tracking  process  focuses  on  a 
stationary  ground  target  and  is  able  to  estimate  the  range  from  the  SUAV  to  the 
target.  In  the  case  of  a  moving  ground  target,  the  current  tracking  process  does 
not  yield  information  on  the  speed  and  direction  which  the  target  is  traveling. 

B.  PROBLEM  FORMULATION 

The  purpose  of  this  project  was  to  investigate  the  use  of  a  filter  to  estimate 
the  ground  target  speed  and  heading.  In  this  thesis,  the  applications  of  two 
different  filters  were  discussed,  with  regards  to  the  formulation  of  the  filter  and 
also  the  filter  performance  in  tracking  motion. 

In  order  to  assess  the  filter  performance,  existing  SUAV  truth  models  were 
used  to  provide  flight  and  camera  models.  The  main  focus  of  this  project  was  the 
estimation  of  target  speed  and  the  range  from  SUAV  to  target.  The  range  is  an 
important  variable  to  be  estimated,  as  this  has  a  bearing  on  the  guidance  for  the 
SUAV  flight  pattern. 
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II.  LITERATURE  SURVEY 


A.  RANGE  ESTIMATION  FOR  STATIONARY  TARGET 

The  study  and  design  of  the  control  system  using  SUAV  and  onboard 
vision  device  for  tracking  stationary  target  was  previously  carried  out  in  NPS.  In 
this  section  that  follows,  the  technique  of  range  estimation  by  Prince  [Ref  1]  is 
summarized. 

The  range  from  the  SUAV  to  a  ground  target  was  estimated  using  linear 
discrete  Kalman  Filter.  The  range  was  subsequently  used  to  guide  the  SUAV  to 
fly  in  a  circular  path  around  the  target,  so  that  the  SUAV  can  maintain  a  defined 
distance  from  the  target  during  the  tracking  process.  The  Kalman  filtering 
technique  was  employed  to  estimate  the  range  from  SUAV  to  target  and  the 
filter’s  system  equation  was  given  by 

xk+1  =Fxk+wk  [Ref  1  ] 


where  xk 


Pk 

Pk 


F  = 


J  k 


At 

1 


and  wk  ~  N{0,Qk)  is  the  process  noise  with 


covariance  Qk,  At  denotes  the  sample  time  and  p  denotes  the  range  from  SUAV 
to  the  target.  The  measurement  equation  was  given  by 


zk=Hxk+uk  [Ref  1  ] 


where  zk 


v. 


Pk 


H„ 


4 

0 


0 

1 


and  vk~N(0,Rk)  is  the  measurement  noise 


with  covariance  Rk,  Xk  denotes  the  estimated  line-of-sight  (LOS)  rate  and 
Vk  denotes  the  SUAV  velocity  vector  that  is  perpendicular  to,  the  LOS  to  target, 

where  Vk  =  pi.  The  estimated  line-of-sight  rate  Xk  was  obtained  using  another 
set  of  system  and  measurement  equations,  see  Prince  [Ref  1]  for  more  details. 
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Constant  Kalman  gains  Kk+i  could  be  used  in  the  measurement  update 
equation  to  obtain  the  updated  range  estimate.  The  measurement  update  was 
given  by 

x^i  =  xk+1  +  Kk+,  (zk+1  -  Hk+,xk+1 )  [Ref  1  ] 

This  application  of  Kalman  filter  to  this  problem  was  successful  in 
obtaining  the  estimated  range  thus  enabling  the  use  of  range  information  for 
SUAV  flight  path  control  and  for  determining  target  location. 

Another  approach  to  estimate  the  range  to  a  stationary  target  was  done  by 
Wang  et  al  [Ref  2]  using  nonlinear  Kalman  filtering.  The  following  diagram  shows 
the  definitions  of  key  variables  used  in  the  tracking  kinematics. 


X' 


SUAV 


vt 


Figure  1 .  Moving  target  tracking  in  inertial  X|  and  Y|  frame  [After:  Ref  2], 


The  process  model  was  given  by 

x  =  f(x,u /) 

;  [Ref  2] 

z  =  h(x ) 
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where  x  = 


T> 

P 

X 


-X  +  y 

-Vg  sin/7 

2V  sin 7  .  V  sin/7 

— - X — - - y 

P  P 


z  = 


il 

Vg  COS  77 


and  h(x)  = 


V 

pX 


[Ref  2] 


A  series  of  steady  state  Kalman  gains  K  was  computed  based  on  several  range 
p  and  estimation  of  the  range  to  the  moving  target  was  obtained  in  simulation.  In 
the  same  study,  the  theoretical  range  was  obtained  through  derivation  of 
kinematics  relationship  between  SUAV  and  target. 

B.  VELOCITY  ESTIMATION  OF  UNDERWATER  VEHICLE 

In  separate  study  by  Oliveira  et  al  [Ref  3],  an  Autonomous  Surface  Craft 
(ASC)  tracked  the  velocity  of  an  Autonomous  Underwater  Vehicle  (AUV)  and 
estimated  the  velocity  of  the  later  using  a  nonlinear  estimator.  The  nonlinear 
relationship  of  this  tracking  problem  was  solved  based  on  the  theory  of  linear 
parametrically  varying  system.  This  section  describes  the  study  by  Oliveira  et  al 
[Ref  3], 

There  are  two  parts  to  the  solution,  a  process  model  and  a  tracker  design. 
The  process  model  comes  in  the  form: 

p=-^R{X}s{'vs)m  +  b  +  wu 

b  =  0  [Ref  3] 

ym=hA(cp)  +  wy 

where  p  was  the  position  of  the  AUV,  b  was  the  velocity  bias  to  be  estimated. 
The  rotation  matrix  transforming  {S}  to  {1}  frame  is  s'R(/t)  and  w  is  the  noise 

input.  The  measurement  ym  is  given  by  y  =  [uc  vc  z]T ,  hA(cp)  was  the 
mapping  of  the  position  of  AUV,  with  respect  to  camera  frame,  into  the  camera 
image  plane  uc,  oc  and  vertical  height  z  from  ASC  to  AUV.  The  figure  below 
shows  the  relationship  of  ASC/camera  and  AUV,  in  relation  to  the  inertial  frame. 
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ASC:  {S} 


Figure  2.  Tracking  of  underwater  AUV  target  [After:  Ref  3], 

The  key  to  estimating  the  AUV  velocity  is  the  relationship: 
hA(cp)-hA(cp)  =  L(cp,cp)H(cp)(cp-cp)  [Ref  3] 

where  L{cp,c p)  =  diag{zc  / zc,zc  / zc,  1)and  H{cp)  is  the  Jacobian  of  hA(cp).  By 
having  L{cp,c p)  =  1  if  zc  /zc  « 1 ,  the  expression  becomes 

0 CP-°P )  =  H(CP)  1  •  (hA(cp) ~ hA(cp ))  [Ref  3]  (1 ) 

This  expression  relates  errors  in  sensor  measurement  to  errors  in  the 
estimation  variables,  thus  casting  the  estimation  problem  in  linear  parametrically 
varying  system  (LPV)  framework.  The  filter  realization  is  given  by: 

P  =~sRWs{'vs  )m+b  +  K^R(A)H\cp)(hA(cp)  -ym) 

[Ref  3] 

b  =  K2'c  R{A)H\C  p)(hA(c  p)- y  m) 

This  filter  realization  is  also  shown  in  figure  3. 
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S(‘vs) 
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Figure  3.  Tracker  structure  [After:  Ref  3], 

Estimator  gains  K-i  and  K2  were  selected  to  achieve  desired  performance 
for  the  filter. 
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III.  PROBLEM  FORMULATION 


A.  PROBLEM  FORMULATION 

In  the  subsequent  chapters,  two  approaches  to  estimation  of  range  and 
velocity  of  the  moving  ground  target  are  described.  The  first  approach  uses  the 
filter  based  on  LPV  system  as  described  in  chapter  II.  The  second  approach 
attempts  to  estimate  the  target  range  and  velocity  based  on  continuous  nonlinear 
Kalman  filtering  using  steady  state  Kalman  gains.  Both  approaches  will  be 
assessed  through  simulations  using  the  MATLAB  tool. 

1.  Nonlinear  Deterministic  Filter  with  Stability  Guarantee 

The  following  diagram  shows  the  framework  for  SUAV  and  target  in  the 
inertial  frame,  which  will  be  used  in  the  subsequent  velocity  estimation. 


Velocity: 

SUAV  '(Vg)  Camera 


The  approach  to  estimate  target  range  and  velocity  requires  measurement 
inputs  from  the  tracking  errors  uc  and  uc,  as  well  as  altitude  z  and  SUAV  velocity. 
Altitude  was  assumed  to  be  obtained  from  the  SUAV  altimeter.  The  velocity  was 
assumed  to  be  obtainable  from  GPS  based  computation. 
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For  the  purpose  of  this  study,  an  already  existing  UAV  truth  model  was 
used  to  provide  the  required  inputs  to  the  filter.  The  filter  was  assessed  for  its 
ability  to  track  the  target  with  and  without  the  addition  of  measurement  noise. 

2.  Filter  Based  on  Kalman  Filtering  Technique 
The  following  diagram  shows  the  framework  for  SUAV  and  target  motion 
in  the  inertial  frame.  Here  the  framework  is  two  dimensional  as  compared  to  the 
previous  filter  approach  which  was  three  dimensional. 


up 

Figure  5.  Target  tracking  framework  for  Kalman  filter  approach. 

The  Kalman  filter  approach  required  collection  of  measurements  for 
velocity  Vg  and  angle  77.  These  measurements  were  compared  with  the 

estimated  V  and  77 .  Angle  77  is  the  angle  between  the  vector  Vp,  which  is 

perpendicular  to  the  LOS,  and  the  velocity  vector  Vg.  These  values  were 
assumed  to  be  obtainable  from  the  SUAV  flight  data.  An  already  existing  UAV 
truth  model  was  used  to  provide  the  necessary  inputs  to  the  filter.  Together  with 
the  process  model,  which  will  be  discussed  in  more  detail,  the  computed  Kalman 
gain  was  used  to  generate  new  estimates  of  the  state  variables. 
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B.  COORDINATE  SYSTEM 

1.  Camera  Coordinates 

The  camera  frame  is  denoted  by  {C}  and  has  coordinates  Xc,  Yc  and  Zc 
where  Xc  is  the  distance  to  the  target  and  has  its  origin  at  the  camera  pin-hole 
location.  Xc  is  positive  in  the  direction  of  the  target,  along  the  camera  to  target 
axis.  Yc  is  the  lateral  distance  of  the  target  from  the  Xc  axis  and  Zc  is  the  vertical 
distance  of  the  target,  positive  when  pointing  downwards  from  the  Xc  axis.  The  Yc 
and  Zc  position  of  the  target  will  be  represented  as  the  uc  and  oc  offset  distance 

from  the  Xc  axis  respectively,  on  the  camera  image  plane.  The  focal  length  f  of 
the  lens  is  12mm.  Finally  the  camera  is  located  at  a  height  Z  from  the  target,  in 
the  inertial  frame.  The  camera  coordinate  system  is  illustrated  below. 


Figure  6.  Camera  coordinate  system. 


2.  Gimbal  Coordinates 

The  camera  pointing  angles  or  the  gimbal  angles  are  denoted  by  two 
angles  namely  gimbal  pitch  6C  (or  tilt  angle)  and  yaw  (pc  (or  pan  angle).  These 
are  angled  with  respect  to  the  SUAV  airframe  body  coordinate  system. 
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3.  Body  Coordinates 

The  SUAV  airframe  body  frame  is  denoted  by  {B}  and  has  coordinates  XB 
pointing  towards  the  nose  of  the  SUAV,  YB  pointing  to  right  wing  of  SUAV  and  ZB 
pointing  upwards  from  the  SUAV.  The  airframe  is  positioned  in  the  inertial  frame 
and  rotated  by  the  angles  roll  <j)B  ,  pitch  0B  and  yaw  <pB .  These  are  computed  with 

respect  to  the  inertial  frame. 

4.  Inertial  Coordinates 

The  inertial  coordinate  system  is  denoted  by  {1}  and  has  coordinates  X|,  Y| 

and  Z|. 

5.  Transformation  Matrix 

The  rotation  matrix  from  camera  to  body  frame  [Ref  5]  is: 


cos  9C  cos  (pc 

cos^  sin^c 

-  sin<9( 

-sin  <pc 

COS^c 

0 

sin^c 

sin^c  sin^ 

cos  9C 

-sin^ 
cos  0e  sin^e 
cos  0B  COS  (j)B 

Then,  the  rotation  matrices  from  camera  to  inertial  frame  and  vice-versa  are: 
cR=bR-cR  and 

^R=(C'R)7  respectively. 


The  rotation  matrix  from  body  to  inertial  frame  [Ref  5]  is: 

cos  0 B  cos  <pB  cos  0 B  sin  cpB 

J,R  =  I  cos  (pB  sin  0 B  sin  (/>B  -  sin  cpB  cos  <f>B  cos  cpB  cos  +  sin  (pB  sin  0B  sin  (f>B 
cos  (pB  sin  0 B  sin  +  sin  cpB  cos  (j)B  sin  cpB  sin  0B  cos  -  cos  cpB  sin  (f)B 
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IV.  APPLICATION  OF  NONLINEAR  DETERMINISTIC  FILTER 
WITH  STABILITY  GUARANTEE 


A.  DESCRIPTION  OF  FILTER 

The  filter  described  in  Oliveira  et  al  [Ref  3]  was  applied  to  the  moving 
ground  target  tracking  problem  in  this  project.  Instead  of  tracking  the  AUV,  the 
filter  technique  was  used  to  track  the  ground  target.  A  six  DOF  SUAV  truth  model 
from  Lizarraga  [Ref  8]  was  used  to  generate  required  inputs  for  the  filter.  The 
following  sections  describe  the  filter  in  more  detail. 

1.  Process  Model 

The  following  process  model  in  equations  (2)  to  (4)  [Ref  3]  was  used,  in 
absence  of  noise,  to  implement  the  SUAV  to  ground  target  tracking: 

p  =  -(lVg)m+b  (2) 

<6  =  0  (3) 

ym=hAcp)  (4) 


In  equation  (1)  above,  the  first  term  -(Vg)m  refers  to  the  inertial  speed  of 
SUAV.  The  second  term  b  denotes  the  actual  target  velocity  which  the  filter  will 
estimate.  Hence  the  estimated  velocity  of  the  target  Vt  will  be  based  on  b. 
Based  on  the  assumption  that  the  target  is  traveling  at  a  constant  speed  and 
heading,  the  time  derivative  6  is  zero  in  equation  (3).  The  first  term  in  the 
measurement  equation  (4)  converts  the  position  of  target  in  camera  coordinates 
cp,  i.e.,  xc,  yc  and  zc  into  the  image  plane  coordinates  and  altitude  difference  z/ , 
according  to  the  relationship: 


y  = 


Un 


f-yc 

fz „ 


R13  '*0+^23  'y°  +R 


33 


[Ref  3] 


(5) 
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where  f  is  12mm,  R12,  R23  and  R33  are  the  elements  of  the  third  column  of  the 
rotation  matrix  C,R.  Equation  (5)  represents  the  measurement  equation 

ym  =  hA(cp)- 

The  process  model  was  then  used  to  design  the  filter  following  Oliveira  et 
al  [Ref  3]  with  minor  modification: 

P  =  -('Vg  )m+b  +  KVc r  • H\cp){hA{cp ) - ym )  (6) 

b  =  K2-cR-H\cp){hA{cp)-ym)  (7) 

The  notation  H(cp)  refers  to  the  Jacobian  of  hA(cp)  and  H  \cp)  is  the 
inverse  of  H(cp).  The  selection  of  gains  Ki  and  K2  will  be  described  in  the  next 
section.  The  resulting  implementation  is  shown  below: 


(V  ) 

\  v  g  )m 


Figure  7.  Tracker  structure  [After:  Ref  3], 

2.  Gain  Selection 

Using  identity  (1 )  and  assuming  xc  =xc,  the  filter  dynamics  are  given  by 
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Figure  8.  Basic  position  model. 


By  implementing  the  observer  according  to  Ogata  [Ref  7],  which  has  the  form 


x  =  Ax  +  Bu  +  L(ym  -  Cx) ,  where  A= 


"o  r 

,  B  = 

"0" 

0  0 

/ 

,  C  =  [/  0]„  L  = 


with 


Ki  and  K2  being  the  gains  shown  in  the  filter  structure,  Ki  and  K2  can  be  found  by 
pole  placement  technique  as  described  in  Ogata  [Ref  7],  The  poles  selected 
were  [-3  -3  -3  -1  -1  -1]  and  the  resulting  gains  were: 


*i  = 

"-4 

0 

0 

-4 

0  " 

0 

and  K2  = 

"-3 

0 

0 

-3 

0  ‘ 

0 

0 

0 

-4  1 

0 

0 

-3 

These  were  the  initial  gains  used  to  assess  the  filter  performance.  The  gains 
were  subsequently  varied  to  address  noise. 

B.  RESULTS  AND  DISCUSSION 

1.  Filter  Performance  in  Absence  of  Noise 

The  following  graphs  show  the  performance  of  the  filter  when  the  SUAV 
truth  model  was  configured  such  that  the  SUAV  attempts  to  circle  around  a 
moving  target  traveling  at  velocity  [10,  5,  0]  m/s  in  the  inertial  frame,  along  xi,  yi 
and  Z|  axis. 

The  results  showed  that  the  convergence  of  estimated  target  velocity  was 
achieved  in  five  seconds  in  absence  of  measurement  noise. 


15 


Velocity  (m/s) 


12 

10 

»  8 

1 

5  6 

o 

o 

£  4 
2 
0 


Velocity  of  target  in  X  direction 


10  20  30  40  50  60 

time  (s) 


70 


80 


-afa- 

90 


100 


6 

5 

To  4 

1 

3 

'o 

o 

£  2 
1 
0 


Velocity  of  target  in  Y  direction 


.1 _ L  . 


I 


_l _ I _ 1 _ L_ 


10  20  30  40  50  60  70  80  90  100 

time  (s) 


0.02 

0.01 

0 

-0.01 
-0.02  1 


0 


Velocity  of  target  in  Z  direction 


i  i  i  r 


i  i  i  r 


“I - 1 - 1 - r 


“i - 1 - 1 - r 


_ j _ I _ 1 _ I _ I _ I _ I _ I _ I _ 

10  20  30  40  50  60  70  80  90 

time  (s) 


100 


Figure  9.  Estimated  target  velocity  in  inertial  frame. 
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Figure  10.  Target  position  error  in  inertial  frame. 
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Figure  1 1 .  Range  from  SUAV  to  target. 
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Figure  12.  Estimated  target  heading. 


90  100 


The  target  heading  was  computed  from  the  angle  resulting  from  the  vector 
summation  of  the  estimated  target  velocity  components  along  xi  and  yi  axis.  The 
estimated  target  heading  corresponded  well  with  the  true  target  heading  of  26.6 
degrees. 

By  reducing  the  gains  Ki  and  K2,  it  was  observed  that  the  convergence  for 
the  estimated  state  variables  were  slower.  In  the  example  involving  velocity,  the 
convergence  was  around  60s.  The  gains  as  a  result  of  pole  selection  of  [-0.3  -0.3 
-0.3  -0.1  -0.1  -0.1],  were  as  follows. 


-0.4 

0 

0 

-0.03 

0 

0 

*1  = 

0 

-0.4 

0 

and  K 2  = 

0 

-0.03 

0 

0 
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-0.4 
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0 

-0.03 
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Velocity  of  target  in  X  direction 
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Figure  13.  Example  of  velocity  estimation  with  lower  gains. 


2.  Filter  Performance  with  Noisy  Measurements 

The  following  graphs  showed  the  estimation  of  target  motion  when  zero 
mean  white  noise  was  added  to  the  measurements.  The  rms  in  velocity  channel 
was  ±2  m/s,  camera  pan/tilt  rms  was  ±0.3  degrees,  SUAV  euler  angles  rms  was 
±  2.8  degrees,  image  plane  error  uc  &  vc  rms  was  ±5  degrees,  and  height  z\  rms 
from  SUAV  to  target  was  ±9  m. 
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Velocity  of  target  in  X  direction 


Velocity  of  target  in  Y  direction 


Velocity  of  target  in  Z  direction 


Figure  14.  Estimated  velocity  in  inertial  frame  (with  measurement  noise). 
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Figure  1 5.  Mean  of  target  velocity  error. 
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Figure  16.  Standard  deviation  of  target  velocity  error. 
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Position  error  of  target  in  Xdirection 
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Figure  17.  Target  position  error  (with  measurement  noise). 
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Figure  18.  Mean  of  target  position  error. 
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Figure  19.  Standard  deviation  of  target  position  error. 
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Figure  20.  Range  from  SUAV  to  target  (with  measurement  noise). 
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Target  heading 


The  convergence  for  the  estimates  was  now  around  25  seconds  for 
velocity  (within  90%  of  true  velocity),  based  on  lower  gains  setting  by  selecting 
the  poles  [-0.3  -0.3  -0.3  -0.1  -0.1  -0.1  ].  Higher  gains  tended  to  cause  wider 
fluctuations  in  the  estimation  of  target  motion.  Thus,  reduction  in  gains  also  has 
the  effect  of  reducing  the  fluctuations  in  the  estimation  of  target  motion. 

In  conclusion,  this  filter  worked  well  in  simulation  in  the  presence  of  white 
noise.  There  was,  however,  a  balance  required  between  fast  convergence  time 
and  degree  of  fluctuations  in  the  target  motion  estimates.  This  can  be  achieved 
by  selecting  appropriate  poles,  hence  the  gains  Ki  and  K2. 
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V.  APPLICATION  OF  KALMAN  FILTER 

A.  DESCRIPTION  OF  FILTER 

The  next  filter,  used  to  estimate  target  motion,  employed  the  continuous 
nonlinear  Kalman  filtering  technique  described  in  Grewal  et.  al.  [Ref  6],  Before 
the  filter  can  be  implemented,  the  kinematics  of  the  tracking  problem  must  be 
established. 

1.  Kinematics  Equations 

The  following  diagram  showed  the  relations  between  the  SUAV  and 
moving  target. 


SUAV 


X' 


Figure  22.  Target  tracking  in  inertial  frame  [After:  Ref  2], 

The  state  variables  x  comprise  the  parameters  77,  p.  A,  Vt  and  i//t.  The 

target  was  assumed  to  be  moving  with  constant  velocity  and  heading.  The 
resulting  kinematics  relationship  was  as  follows: 


n  1 

rl  =  ^~h  +  Vg 

[Ref  2] 

(8) 

V  =  Yg  -A 
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[Ref  2] 

(9) 

P  =  -Vgs\n>i  +  Vts\n{y/t  +  r/~¥g) 

[Ref  2] 

(10) 

j  _  Vg  cos r)  Vtcos (¥t+r/-¥g) 

(11) 

p  p 

Letting  Vt  =  0  ,  y/t  =  0  and  assuming  Vg  =  0  ,  we  obtain 


Vsinrj.  V gs\nr]  Vcosi 7.  Vt  s\n(y/t  +  77 -y/  )  Vtcos  (y/t  +  rj~¥g)  . 

1  =  — g- - y/g  + - A--2— j — P - —yi  + - 5 - —p 


p  p  p  p  p 

(12) 

Vt=0  (13) 

¥t=  0  (14) 

2.  Process  Model 

The  nonlinear  process  model  was  obtained  from  Grewal  et  al  [Ref  6]  as 
follows: 

x(t)  =  f(x(t),t)  +  w(t )  w(t)  ~  N(0,Q(t))  (15) 

z(t)  =  h(x(t),t)  +  v(t)  v(t)~N(0,R(t))  (16) 

The  implementation  equations  [Ref  6]  were: 

z(t)  =  h(x(t),t)  (18) 

The  linear  approximation  equations  [Ref  6]  were: 


F(fy,  df(X,t) 

dx  x=x(t ) 

(19) 

Hyy.  dh(X,t) 

dx  x=x(t) 

(20) 

The  Kalman  gain  equations  [Ref  6]  were: 

P(t)  =  F(t)P(t)  +  P(t)FT(t)  +  G(t)Q(t)GT(t)-mR(t)KT(t)  (21) 

K(t)  =  P(t)HT(t)R  \t)  (22) 
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3.  Kalman  Gain  Computation 

F(t)  was  computed  based  on  the  assumption  of  the  following  constant 
values:  Vg  =  20  m/s,  77  =  0,  range  p  =  500  m,  X  = =  0.04  rad/s,  Vt  =  5  m/s, 

\j/g  =  0.04  rad/s.  y/g  took  on  the  latest  value  from  the  SUAV  truth  model  as  the 
SUAV  changes  heading.  y/t  was  unknown  and  hence  took  on  the  value  from 
latest  estimated  target  heading  y7t . 


In  the  measurement  equation  (16),  h{t)  comprised  measurements  [ 77  V  ] 
where  Vg  «  pX  . 


To  obtain  steady  state  gain,  equation  (21)  was  set  to  zero,  i.e. ,  letting 
P{t)  =  0 .  The  gain  from  equation  (22)  was  finally  obtained  by  solving  Algebraic 
Riccati  Equation  [Ref  2]  for  P. 


The  process  noise  covariance  Q  and  measurement  noise  covariance  R 
were  chosen  as  follows: 


Q  = 


0.0001 

0 

0 

0 

0 
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0.0001 
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0  0 

0.0001  0 

0  0.0101 
0  0 
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0.0001 


R  = 


0.0005 

0 


0 

0.025 


Using  nominal  values,  Vg  =  20  m/s,  77 


=  0,  range  p  =  500  m,  i  = 


0.04  rad/s,  Vt  =  5  m/s  and  y'/g  =  0.04  rad/s,  the  heading  difference  between  the 
SUAV  and  target  was  varied  over  a  cycle  of  360  degrees.  With  these  inputs  to 
F(t),  the  steady  state  Kalman  gain  K  was  computed  as  described  above  and  its 
values  are  shown  below. 
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Figure  23.  Kalman  gain. 

In  the  above  figure,  the  gain  was  denoted  by  K;j  where  the  subscript  /  =  1 
to  5  was  associated  with  the  respective  state  variables  rj ,  p ,  A,  Vt  and  y/t  in 
that  order.  The  subscript 7  =  1  to  2  was  associated  with  the  measurements  77  and 
Vg  ~  pA  respectively.  Clearly,  the  gain  varied  according  to  the  difference  in 
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error  in  q  (rad)  q  (rad) 


heading  between  SUAV  and  target.  This  set  of  gains  was  used  to  provide 
estimates  of  the  state  variables. 

B.  RESULTS  AND  DISCUSSION 


Equation  (17)  was  implemented  using  existing  simplified  UAV  truth  model 
based  on  only  one  body  turn  rate  \j/g  in  yaw,  for  the  airframe.  The  filter 

performance  is  shown  in  figures  below. 
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Figure  24.  Estimated  77. 
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Figure  25.  Estimated  range  from  SUAV  to  target. 

The  error  was  about  40m  (after  500  seconds)  compared  to  the  true  range 
of  approximately  200m  mean,  meaning  an  error  of  about  20%. 
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Estimated  target  velocity 
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Figure  26.  Estimated  target  velocity  Vt . 

The  target  velocity  estimate  was  not  accurate  enough.  It  was  about  twice 
the  true  target  velocity  based  on  a  target  velocity  of  [2,  2,  0]  m/s  in  the  inertial 
frame  along  xi,  yi  and  z\  axis,  i.e. ,  2.83  m/s.  at  a  heading  of  45  degrees  or  0.785 
rad.  The  large  discrepancy  between  the  true  and  estimated  velocity  could  not  be 
successfully  remedied.  Possible  causes  could  be  due  to  the  choice  of  nominal 
values  assumed  for  variables  in  H(t)  and  the  low  values  of  the  Kalman  gain,  for 
target  velocity,  which  was  related  to  the  choice  of  noise  covariance. 
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Estimated  target  heading 
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Figure  27.  Estimated  target  heading  y/t . 


The  mean  heading  error  was  about  0.06  rad  average  compared  with  the 
true  heading  of  0.785  rad. 


Overall,  the  filter  in  this  particular  implementation  could  provide  estimates 
of  the  state  variables  to  within  20%  error  approximately  except  for  the  target 
velocity.  In  future,  further  assessment  using  Kalman  filter  technique  will  be 
beneficial  in  identifying  the  cause  of  the  estimation  discrepancies  observed 
above. 
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VI.  CONCLUSION  AND  RECOMMENDATIONS 


In  the  tracking  of  stationary  ground  targets,  previous  work  by  various 
authors  was  discussed.  With  the  background  gathered  from  both  the  stationary 
and  moving  targets  tracking,  this  project  attempted  to  apply  known  filtering 
techniques  to  estimation  of  ground  target  range  and  velocity.  In  this  thesis,  the 
problem  of  tracking  moving  ground  target  using  a  camera  mounted  on  a  SUAV 
was  assessed  in  simulation,  using  two  different  filters. 

The  first  filter  was  a  nonlinear  deterministic  filter  with  stability  guarantee. 
This  technique  was  found  to  estimate  the  target  motion  with  fast  convergence  in 
the  region  of  five  seconds,  in  the  absence  of  measurement  noise.  With  white 
noise  in  the  measurement,  the  convergence  time  was  around  25  seconds,  using 
the  gain  described  in  this  thesis.  The  estimates  from  this  filter  compared  very  well 
with  the  true  target  motion. 

The  second  filter  technique  assessed  was  based  on  the  continuous 
nonlinear  Kalman  filter  with  steady  state  gain.  This  approach  could  not  estimate 
closely  the  true  target  motion,  for  the  case  of  this  particular  project,  when 
compared  with  the  first  filter  technique  mentioned  above. 

Overall,  the  attempt  to  estimate  the  moving  ground  target  motion  was 
successful  using  the  first  filtering  technique.  Future  work  is  still  required  to  verify 
the  suitability  of  this  filter  using  real  flight  test  data. 

It  is  recommended  for  future  work,  that  actual  SUAV  flight  and  target 
tracking  data  be  used  to  verify  the  effectiveness  of  the  nonlinear  deterministic 
filter  with  stability  guarantee.  It  is  further  recommended  that  the  Kalman  filtering 
technique  be  studied  further  to  explore  the  issues,  surrounding  estimation  of 
target  motion,  encountered  in  this  project. 
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